#ifndef _QUATERNION_
#define _QUATERNION_

#include <stdio.h>
#include <iostream>
#include <math.h>
#include <vl/VLd.h>
using namespace std;

#define EPSILON 1.0e-6

class Quaternion{
public:
	double w;
	double x;
	double y;
	double z;

	Quaternion(){x=y=z=0; w=1.0;};	// constructor
	Quaternion(double _w, double _x, double _y, double _z);
	Quaternion(double _w, Vec3d _v);
	Quaternion(double _w, Vecd _v);
	Quaternion(Vec4d v);
	Quaternion(Vecd v);
	Quaternion(Vec3d axis, double angle);

	double& operator[] (int i);	// indexing operator
	void operator = (Quaternion);	// assign one to other
	Quaternion operator + (Quaternion);	// operators on Quaternions
	Quaternion operator - ();
	Quaternion operator * (Quaternion);
	Quaternion operator * (double);	// multiplies quat with a scalar
	Quaternion operator / (double);	// divides the quat by a number
	double dot (Quaternion);	// dot product of two Quaternions

	Quaternion conjugate();	// returns the conjugate
	double norm();	// returns the norm of the quat
	void normalize();	// normalize the quat
	Quaternion inverse();	// returns the inverse
	Vec3d getAxis();	// returns axis of rotation
	double getAngle();	// returns angle of rotation
	void axisAngle(double _nx, double _ny, double _nz, double _theta);	// computes based on input angle and axis
	void axisAngle(Vec3d axis, double angle);

	inline Vec4d getVec(){return Vec4d(w,x,y,z);}
	inline Vecd getVecd(){return Vecd(4, w,x,y,z);}
	
	Mat3d getMatrix();	// returns the corresponding rotation matrix
	Mat3d getDerivativeMatrix(int el);	// get the derivative of rotation matrix wrt el no.
	Mat3d getDerivativeMatrix(int el1, int el2);	// get the derivative of rotation matrix wrt el nos. el1 and el2

	void display();	// prints the 4 vector
	
	~Quaternion(){};	// destructor
};


Vec3d RotatePoint(Quaternion q, double x, double y, double z);
Vec3d RotatePoint(Quaternion q, Vec3d pt);	// rotates a point with the Quaternion
Quaternion Slerp(Quaternion p, Quaternion q, double t);
#endif
